/**
  ******************************************************************************
  * @file    :MG216.c
  * @author  :MG Team
  * @version :V1.0
  * @date
  * @brief   : for extended adv and AoA
  ******************************************************************************
***/

/* Includes ------------------------------------------------------------------*/
#include "Includes.h"
#include "math.h"
#include "mg_adv_par_cfg.h"
#include "ble.h"



//standard value
static u32 g_N32 = 11;
static u32 g_N16M = 343750;//@32768hz


u8 g_adi_sid = 0x00;    //rang[0x00,0x0F]     !!!one may change it
u16 g_adi_did = 0x678;  //range[0x00,0xFFF],  !!!one may change it

u8 adva_enabled = 1;
u8 adv_tx_power_enabled = 0;
u8 TxPowerV;

u8 *ble_addr;
u8 ACAD2[] = {0x05,0x09,0x6d,0x67,0x6d,0x67};
u8 ACAD2_len = 0;


//u8  gc_adv_data_n_[] = { 0x02, 0x01, 0x06, 0x05, 0x09, 0x6d, 0x67, 0x30, 0x34};

u8 g_pkt_types = 0;

uint8_t temp_buf[4];
uint8_t data_buf[9];
u8 Ptr_info[3];
//BB RC calibration functions
/*
C32' = C32 *  N32 / N16M  * 32000(31250 for 32768)
N32 --- cycle_32k_reg[3:0]   --> 2^4
N16 --- cycle_16m_reg[12:8] cycle_16m_reg[7:0] cycle_frac_reg[0:5]  --> 2^19

For slave role, we can use the simple algorithm(low accuracy)
C32' = C32 *  N32 / N16M'  * 500(31250/64 for 32768)
N16M' --- cycle_16m_reg[12:8] cycle_16m_reg[7:0] --> 2^13
For 2s interval, there has max of 8 32k-clock accuracy.
*/

//causion: here has the accuracy of 500ppm result!!!!

u32 DoRcCal(u32 In/*in rc value*/, unsigned char *DelayFlagBit/*0 or 10us*/) //return calibated rc value
{    
    u32 Temp=0;
    *DelayFlagBit = 0; //default is 0, [xx-10us] ceiling algorithm may be used and round may lead to 0x80 if decimal part(fractional) is less than 10us.

    {
        //do the rc calibration if any
        //radio_get_RcCalPar(&g_N32,&g_N16M); 
        MG_Read_Buffer(RC_CAL_REG, temp_buf, 4);
        
        g_N32 = temp_buf[1] & 0x1F;
        
        g_N16M  = temp_buf[3];
        g_N16M <<= 8;
        g_N16M |= temp_buf[2];
        g_N16M <<= 6;
        g_N16M |= (temp_buf[0] & 0x3F);
        
        Temp = In * g_N32 * 500 / ((g_N16M + 0x20) >> 6);//Rc_Cal_Simple(In,g_N32,g_N16M);
    }
    
    return Temp;
}

void set_adv_interval(void)//set normal adv interval,[g_adv_interval]unit: 1ms
{
    u8 delay_flag;
    u32 interval = DoRcCal(LF_TIME1MS * g_adv_interval,&delay_flag);
    
    #define RC_LOAD    0x01
    
    MG_Read_Buffer(ADV_INTERV_RC_CFG,data_buf,4);    
    
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval>>8) & 0xff;
    data_buf[2] = (interval>>16) & 0xff;
    data_buf[3] |= RC_LOAD;
    
    MG_Write_Buffer(ADV_INTERV_RC_CFG,data_buf,4);
}

void fill_fifo_data(unsigned char fifo_id/*0:ext,1:chain/sync*/,unsigned char* data, unsigned char len)
{
    MG_Write_Buffer(W_TX_FIFO1 + fifo_id*2, data,len);
}

//without ACAD part
u8 get_extended_header_len(u8 flag)
{
    u8 i,len = 1;
    const unsigned char HDR_Item_Len[] = {6,6,1,2,3,18,1};
    
    for(i = 0 ; i < 7 ; i ++)
    {
        if(flag & (0x01 << i)) len += HDR_Item_Len[i];
    }
    return len;
}

u8* get_auxPtr_par(u32 offset/*unit ms*/,u8 ch_idx,u8 phy_id/*0-1Mhz, 2-coded*/)
{    
    u8 t;    
    u8 CA = 0; //0-:500ppm, 1-:50ppm
    //30~245,700us-30us 2.457s-300us
    
    t = CA << 6;    
    t |= ch_idx;    
    if(offset > 245)
    {
        t |= 0x80;        
    }
    Ptr_info[0] = t;
    
    
    if(offset > 245)
    {
        offset *= 10;
        offset /= 3;
    }
    else
    {
        offset *= 100;
        offset /= 3;
    }    
    Ptr_info[1] = offset & 0xff;
    
    Ptr_info[2] = offset >> 8;
    Ptr_info[2] |= (phy_id << 5);
    
    return Ptr_info;
}

u8 is_data_updated(void)
{
    u8 tmp;
    
    MG_Read_Buffer(WR_SYNC_PKT_STATE,&tmp,1);
    
    //clear the bit if any(write 0 to clear the bit)
    if(tmp & 0x02)
    {
        tmp ^= 0x02;//clear the bit
        MG_Write_Buffer(WR_SYNC_PKT_STATE,&tmp,1);
        return 0x02; //fifo update success
    }
    
    return 0;//fifo update fail
}

u8 update_adv_ext_ind_data(unsigned char* data, unsigned char len)
{
    u8 hd[5];
    u8 hd_ext[4];
    u16 adv_len;
    u8 rv = len;
    
    MG_Read_Buffer(ADV_PKT_CFG,hd,5);
    MG_Read_Buffer(EXT_HEADER_CFG1,hd_ext,4);
    
    adv_len = (hd_ext[2] & 0x3F) + 1 + len;
    hd[0] = AUX_ADV_IND_BIT; //lock bit
    
    if(adv_len > 255)//buffer is overflow
    {
        rv = len - (adv_len - 255);
        adv_len = 255;
    }    
    hd[4] = adv_len;
    
    while(1)
    {
        MG_Write_Buffer(ADV_PKT_CFG,hd,5);
        fill_fifo_data(0,data,len);
        //check reg status
        if(is_data_updated())break;
    }
    
    return rv;//updated data size if any
}

u8 update_aux_chain_ind_data(unsigned char* data, unsigned char len)
{
    u8 hd[7];
    u8 hd_ext[6];
    u16 adv_len;
    u8 rv = len;
    
    MG_Read_Buffer(ADV_PKT_CFG,hd,7);
    MG_Read_Buffer(EXT_HEADER_CFG1,hd_ext,6);
    
    adv_len = (hd_ext[4] & 0x3F) + 1 + len;
    hd[0] = AUX_CHAIN_IND_BIT; //lock bit
    
    if(adv_len > 255)//buffer is overflow
    {
        rv = len - (adv_len - 255);
        adv_len = 255;
    }
    
    hd[6] = adv_len;
    
    while(1)
    {
        MG_Write_Buffer(ADV_PKT_CFG,hd,7);
        fill_fifo_data(1,data,len);
        //check reg status
        if(is_data_updated())break;
    }
    
    return rv;//updated data size if any
}

void update_adv_ext_ind_par(void)//interval,AuxPtr [g_interval_2_aux_step,g_interval_2_aux_step_num]
{
    u8 i;
    u8 delay_flag;
    u32 interval;
    
    set_adv_interval();//set/refresh interval RC
    
    i = 0;
    interval = DoRcCal(g_interval_2_aux_step[i] * LF_TIME1MS,&delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval>>8) & 0xff;
    data_buf[2] = (interval>>16) & 0xff;
    data_buf[3] = (g_interval_2_aux_step_num << 1) | 0x01 | delay_flag;
    
    if(g_aux_ch_sel == MG_ADV_CH_AUTO)
        data_buf[7] = (AUX_ADV_IND_HOP << 1);//hop
    else
        data_buf[7] = 0;
    
    if(g_interval_2_aux_step_num > 1)
    {
        interval = DoRcCal(g_interval_2_aux_step[i+1] * LF_TIME1MS,&delay_flag);
        data_buf[4] = interval & 0xff;
        data_buf[5] = (interval>>8) & 0xff;
        data_buf[6] = (interval>>16) & 0xff;
        data_buf[7] |=  (0x01 | delay_flag);
    }
    MG_Write_Buffer(OFFSET_RC_CFG1, data_buf,8);
    
    if(g_interval_2_aux_step_num <= 2)return;
    
    for(i = 2 ; i < g_interval_2_aux_step_num ; i += 2)
    {
        interval = DoRcCal(g_interval_2_aux_step[i] * LF_TIME1MS,&delay_flag);
        data_buf[0] = interval & 0xff;
        data_buf[1] = (interval>>8) & 0xff;
        data_buf[2] = (interval>>16) & 0xff;
        data_buf[3] = 0x01 | delay_flag;
        
        if(i < g_interval_2_aux_step_num - 1)
        {
            interval = DoRcCal(g_interval_2_aux_step[i+1] * LF_TIME1MS,&delay_flag);
            data_buf[4] = interval & 0xff;
            data_buf[5] = (interval>>8) & 0xff;
            data_buf[6] = (interval>>16) & 0xff;
            data_buf[7] = 0x01 | delay_flag;
        }
        MG_Write_Buffer(OFFSET_RC_CFG1 + (i>>1), data_buf,8);
    }    
}

void set_adv_ext_ind_par(u8 adv_type,u8 *acad2,u8 acad2_len) //AuxPtr [g_interval_2_aux_step,g_interval_2_aux_step_num]
{
    u8 i;
    u8 flags;
    u8 adv_len;
    u8 hd[6+1];
    u8 hd_ext[6];
    u8 tmp[14];
    
//#define ADV_EXT_IND_ONLY /*if adv_ext_ind ONLY, open this macro definition to disable aux_adv_ind package.*/
    
    if(!((g_pkt_types & AUX_SYNC_IND_BIT) || (g_pkt_types & AUX_CHAIN_IND_BIT)))//extended_ind+aux_ind case
    {
        hd[0] = DISABLE_LOCK_DATA_BIT; //unlocked flag
        
        //first package's info
        flags = (HDR_FLAG_ADI | HDR_FLAG_AUXPTR); //xHDR_FLAG_TXPOWER, xHDR_FLAG_TARGETADDR
//        if(g_ext_phy_sel == MG_ADV_PHY_1M)flags |= HDR_FLAG_ADVA;
#ifdef ADV_EXT_IND_ONLY
        if(g_adv_ext2_type == MG_TYPE_ADV_NOT_USED)
        {
            flags &= ~(HDR_FLAG_AUXPTR|HDR_FLAG_ADI);
            flags |= HDR_FLAG_ADVA;
        }
#endif        
        adv_len = get_extended_header_len(flags);//MUST NO ACAD part,NO AdvData  ===> Extended Header Length
        
        hd_ext[0] = adv_len; //non-connectable non-scanable
        hd_ext[1] = flags;
        
        adv_len ++;
        hd[0+1] = adv_type;
        hd[1+1] = adv_len;
        hd[4+1] = 0x0F;
        
        //second package's info
        flags = HDR_FLAG_ADI;
//        if((g_aux_phy_sel == MG_ADV_PHY_500K) || (g_aux_phy_sel == MG_ADV_PHY_125K))
        {
            if(adva_enabled)flags |= HDR_FLAG_ADVA; //HDR_FLAG_TXPOWER,HDR_FLAG_TARGETADDR
            if(adv_tx_power_enabled)flags |= HDR_FLAG_TXPOWER;
        }
        adv_len = get_extended_header_len(flags);//may ACAD part added if any.  ===> Extended Header Length
        if(acad2_len && acad2)adv_len += acad2_len;
        
        hd_ext[2] = adv_len;
        hd_ext[3] = flags;
        
        adv_len ++;
        if((255 - adv_len) >= g_adv_data_pkt2_len)
        {
            adv_len += g_adv_data_pkt2_len;
        }
        else//shorten the adv data if any(buffer overflow)
        {
            adv_len = 255;
        }
        hd[2+1] = adv_type;
        hd[3+1] = adv_len;
#ifdef ADV_EXT_IND_ONLY
        if(g_adv_ext2_type == MG_TYPE_ADV_NOT_USED)
        {
            hd[2+1] = 0x0F; 
        }
#endif
        MG_Write_Buffer(ADV_PKT_CFG, hd,5+1); //adv header
        MG_Write_Buffer(EXT_HEADER_CFG1, hd_ext,4);//extended header
        
        //acad data if any
        if(acad2_len && acad2)MG_Write_Buffer(EXT_HEADER_CFG4, acad2,acad2_len);
        
        //aux adv ind data
        fill_fifo_data(0,g_adv_data_pkt2,g_adv_data_pkt2_len);
    }
    
    for(i = 0 ; i < g_interval_2_aux_step_num ; i ++)
    {
        u8 *Ptr;
        u8 phy = g_aux_phy_sel;
        u8 _ch = g_aux_ch_sel;
        
        if((phy == MG_ADV_PHY_500K) || (phy == MG_ADV_PHY_125K))phy = 2;
        else if(phy == MG_ADV_PHY_1M) phy = 0;
        else phy = 1; //2M
        if(_ch == MG_ADV_CH_AUTO)_ch = 0; //start from channel 0, heihei
        
        Ptr = get_auxPtr_par(g_interval_2_aux_step[i],_ch,phy);
        //set the leading steps value and tuned RC nunbers
        if(i == 0)//first one parameter
        {
            u8 tt[4];
            tt[0] = Ptr[0];
            tt[1] = Ptr[1];
            tt[2] = Ptr[2];
            tt[3] = TxPowerV; //also congfig the tx power value
            //MG_Write_Buffer(EXT_HEADER_CFG3, Ptr,3);
            MG_Write_Buffer(EXT_HEADER_CFG3, tt,4);
        }
        else
        {
            tmp[2*i-2] = Ptr[1];
            tmp[2*i+1-2] = (Ptr[2] & 0x1F) | (Ptr[0]&0x80);
        }
    }
    if(g_interval_2_aux_step_num > 1)//write into the regs except the first one(saved in [SYNC_INTERVAL_RC_CFG])
    {
        MG_Write_Buffer(AUX_OFFSET2_8, tmp,(g_interval_2_aux_step_num*2)-2);
    }
    
    update_adv_ext_ind_par();//update rc(interval included)
}


void set_radio_chain_hop(void)
{
    MG_Read_Buffer(OFFSET_RC_CFG1,data_buf,8);
    
    data_buf[8] = 0;
    if(g_chain_ch_sel == MG_ADV_CH_AUTO)data_buf[8] = AUX_CHAIN_IND_HOP;
    
    MG_Write_Buffer(OFFSET_RC_CFG1,data_buf,9);
}

void update_aux_chain_ind_par(void)//offset rc
{    
    u8 delay_flag;
    u32 interval;
    
    interval = DoRcCal(g_chain_offset * LF_TIME1MS,&delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval>>8) & 0xff;
    data_buf[2] = (interval>>16) & 0xff;    
    data_buf[3] = (0x01 | ( (delay_flag) ? 0x02 : 0x00) );//bit 1 is now means delay flag bit.....
    
    MG_Write_Buffer(SYNC_INTERVAL_RC_CFG, data_buf,4);
}

void set_aux_chain_ind_par(u8 adv_type,u8 *acad2,u8 acad2_len)//[g_chain_offset,g_chain_ch_sel]
{
    u8 flags;
    u8 adv_len;
    u8 hd[6+1];
    u8 hd_ext[6];
    
    u8 phy = g_aux_phy_sel;
    u8 *Ptr;
    
    hd[0] = DISABLE_LOCK_DATA_BIT; //unlocked flag
    
    //first pkt
    flags = (HDR_FLAG_ADI | HDR_FLAG_AUXPTR); //xHDR_FLAG_TXPOWER, xHDR_FLAG_TARGETADDR
//    if(g_ext_phy_sel == MG_ADV_PHY_1M)flags |= HDR_FLAG_ADVA;
    adv_len = get_extended_header_len(flags);//MUST NO ACAD part,NO AdvData  ===> Extended Header Length
    
    hd_ext[0] = adv_len;
    hd_ext[1] = flags;
    
    adv_len ++;
    hd[0+1] = adv_type;
    hd[1+1] = adv_len;
    
    //second pkt's info
    flags = HDR_FLAG_ADI | HDR_FLAG_AUXPTR;
//    if((phy == MG_ADV_PHY_500K) || (phy == MG_ADV_PHY_125K))
    {
        if(adva_enabled)flags |= HDR_FLAG_ADVA; //HDR_FLAG_TXPOWER, HDR_FLAG_TARGETADDR
        if(adv_tx_power_enabled)flags |= HDR_FLAG_TXPOWER;
    }
    adv_len = get_extended_header_len(flags);//may ACAD part added if any.  ===> Extended Header Length        
    if(acad2_len && acad2)adv_len += acad2_len;
    
    hd_ext[2] = adv_len;
    hd_ext[3] = flags;
    
    adv_len ++;
    if((255 - adv_len) >= g_adv_data_pkt2_len)
    {
        adv_len += g_adv_data_pkt2_len;
    }
    else//buffer overflow
    {
        adv_len = 255;
    }
    hd[2+1] = adv_type;
    hd[3+1] = adv_len;

    //third pkt, chain
    flags = HDR_FLAG_ADI; //HDR_FLAG_TXPOWER
    if(adv_tx_power_enabled)flags |= HDR_FLAG_TXPOWER;
    adv_len = get_extended_header_len(flags);//no ACAD part.  ===> Extended Header Length
    hd_ext[4] = adv_len;
    hd_ext[5] = flags;
    
    adv_len ++;
    if((255 - adv_len) >= g_adv_data_pkt3_len)
    {
        adv_len += g_adv_data_pkt3_len;
    }
    else//buffer overflow
    {
        adv_len = 255;
    }
    hd[4+1] = adv_type;
    hd[5+1] = adv_len;
     
    MG_Write_Buffer(ADV_PKT_CFG, hd,6+1); //adv header
    MG_Write_Buffer(EXT_HEADER_CFG1, hd_ext,6);//extended header
        
    //acad data if any
    if(acad2_len && acad2)MG_Write_Buffer(EXT_HEADER_CFG4, acad2,acad2_len);
        
    //aux adv ind data
    if(g_adv_data_pkt2_len)fill_fifo_data(0,g_adv_data_pkt2,g_adv_data_pkt2_len);
    //aux chain ind data
    if(g_adv_data_pkt3_len)fill_fifo_data(1,g_adv_data_pkt3,g_adv_data_pkt3_len);
    
    set_adv_ext_ind_par(adv_type,acad2,acad2_len);//generate Extened_ind's Ptr(s),//step values, to be added
    
    //aux_adv_ind-->ptr
    if((phy == MG_ADV_PHY_500K) || (phy == MG_ADV_PHY_125K))phy = 2;
    else if(phy == MG_ADV_PHY_2M)phy = 1;
    else phy = 0;//1M
    Ptr = get_auxPtr_par(g_chain_offset,g_chain_ch_sel == MG_ADV_CH_AUTO ? 0 : g_chain_ch_sel,phy);
    
    //chain ptr
    MG_Read_Buffer(EXT_HEADER_CFG3,data_buf,3); //load the aux_ptr-1 values
    data_buf[3] = TxPowerV;
    data_buf[4] = Ptr[0];
    data_buf[5] = Ptr[1];
    data_buf[6] = Ptr[2];
    data_buf[7] = TxPowerV;
    data_buf[8] = TxPowerV;
    MG_Write_Buffer(EXT_HEADER_CFG3, data_buf,9);
    
    set_radio_chain_hop();
    
    update_aux_chain_ind_par();//update rc
}


u8 ACAD3[10]= {0x11,0x22,0x33,0x44,0x55,0x66,0x77,0x88,0x99,0xaa};
u8 ACAD3_len = 4;
const u8 g_sync_map[5] = {0xff,0xff,0xff,0xff,0x1f};
const u8 g_sync_crc_init[3] = {0xA5,0x69,0x36};
u8 g_sync_aa[4] = {0x12,0x34,0x56,0x78};        //!!!one may random it
u8 g_syncInfo[18];

void set_sync_ind_offset(u32 offset/*ms*/) //!!! It MUST always less than 2.457s ==> Offset Adjust = 0 !!!
{
    u8 unit_value = 0;
    
    //30~245,700us-30us 2.457s-300us
    if(offset > 245)
    {
        unit_value = 0x20;
        
        offset *= 10;
        offset /= 3;
    }
    else
    {
        offset *= 100;
        offset /= 3;
    }
    
    g_syncInfo[0] = offset & 0xff;
    g_syncInfo[1] = offset >> 8;
    g_syncInfo[1] |= unit_value;
}

void set_sync_ind_interval(u16 interval/*ms*/)
{
    //unit change to 1.25ms
    interval *= 4;    
    interval /= 5;
    
    g_syncInfo[2] = interval & 0xff;
    g_syncInfo[3] = interval >> 8;
}

void set_secondary_adv_channel_map(u8* map/*5Bytes*/)
{
    g_syncInfo[4] = map[0];
    g_syncInfo[5] = map[1];
    g_syncInfo[6] = map[2];
    g_syncInfo[7] = map[3];
    g_syncInfo[8] = (map[4]&0x1F);// | (g_syncInfo[8] & 0xE0);
}

void set_sync_ind_aa(u8* aa)
{
    g_syncInfo[9] = aa[0];
    g_syncInfo[10] = aa[1];
    g_syncInfo[11] = aa[2];
    g_syncInfo[12] = aa[3];
}

void set_sync_ind_crc_init(u8* sync_crc_init)
{
    g_syncInfo[13] = sync_crc_init[0];
    g_syncInfo[14] = sync_crc_init[1];
    g_syncInfo[15] = sync_crc_init[2];
}

void set_sync_ind_counter(u16 counter)
{
    g_syncInfo[16] = counter & 0xFF;
    g_syncInfo[17] = counter >> 8;
}

void set_sync_ind_info(void)
{    
    u8 ch = g_aux_ch_sel;
    u8 phy = g_aux_phy_sel;
    u8* Ptr;
    
    if((phy == MG_ADV_PHY_125K) || (phy == MG_ADV_PHY_500K))phy = 2;
    else if(phy == MG_ADV_PHY_2M)phy = 1;
    else phy = 0;//1M
    if(ch == MG_ADV_CH_AUTO)ch = 0; //start from channel 0
    
    //generate sync_info
    set_sync_ind_aa(g_sync_aa);
    set_sync_ind_crc_init((u8*)g_sync_crc_init);
    set_secondary_adv_channel_map((u8*)g_sync_map);
    set_sync_ind_counter(0x64);
    set_sync_ind_interval(g_adv_interval);    
    set_sync_ind_offset(g_sync_offset2_2_sync);
    
    MG_Write_Buffer(SYNC_INFO_CFG1,g_syncInfo,16);
    
    data_buf[2] = g_syncInfo[16];
    data_buf[3] = g_syncInfo[17];
    data_buf[4] = 0x01; //reload
    MG_Write_Buffer(SYNC_INFO_CFG2,data_buf,5);
    
    //generate aux_ind Ptr info
    Ptr = get_auxPtr_par(g_sync_offset1_2_aux,ch,phy);
    data_buf[0] = Ptr[0];
    data_buf[1] = Ptr[1];
    data_buf[2] = Ptr[2];
    data_buf[3] = TxPowerV;
    //no ptr for aux_sync_ind(using SynInfo @ set_sync_ind_offset(...))
    data_buf[7] = data_buf[8] = data_buf[3];
    MG_Write_Buffer(EXT_HEADER_CFG3,data_buf,9);
}

void update_aux_sync_ind_par(void)//set rc vaule
{    
    u8 delay_flag;
    u32 interval;
    
    //g_adv_interval ==> sync interval
    interval = DoRcCal(g_adv_interval * LF_TIME1MS,&delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval>>8) & 0xff;
    data_buf[2] = (interval>>16) & 0xff;
    data_buf[3] = (g_interval_2_sync_step_num << 4) | 0x01;    
    MG_Write_Buffer(SYNC_INTERVAL_RC_CFG, data_buf,4);    
    
    //g_sync_offset1_2_aux,
    //g_sync_offset2_2_sync
    interval = DoRcCal(g_sync_offset1_2_aux * LF_TIME1MS,&delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval>>8) & 0xff;
    data_buf[2] = (interval>>16) & 0xff;
    data_buf[3] = 0x01 | delay_flag;
    interval = DoRcCal(g_sync_offset2_2_sync * LF_TIME1MS,&delay_flag);
    data_buf[4] = interval & 0xff;
    data_buf[5] = (interval>>8) & 0xff;
    data_buf[6] = (interval>>16) & 0xff;
    data_buf[7] = 0x01 | delay_flag;
    if(g_aux_ch_sel == MG_ADV_CH_AUTO)data_buf[7] |= (AUX_ADV_IND_HOP << 1);//hop
    MG_Write_Buffer(OFFSET_RC_CFG1, data_buf,8);
}

void set_aux_sync_ind_par(u8 adv_type, u8 *acad2,u8 acad2_len,u8 *acad3,u8 acad3_len) //[aa,g_adv_interval,g_interval_2_sync_step_num,g_sync_offset1_2_aux,g_sync_offset2_2_sync]
{
    u8 flags;
    u8 adv_len;
    u8 hd[6+1];
    u8 hd_ext[6];
    
    hd[0] = DISABLE_LOCK_DATA_BIT;//unlocked flag
    
    //extended adv ind
    flags = (HDR_FLAG_ADI | HDR_FLAG_AUXPTR | HDR_FLAG_TXPOWER); //xHDR_FLAG_TXPOWER, xHDR_FLAG_TARGETADDR
//    if(g_ext_phy_sel == MG_ADV_PHY_1M)flags |= HDR_FLAG_ADVA;
    adv_len = get_extended_header_len(flags);//MUST NO ACAD part,NO AdvData   ===> Extended Header Length
    hd_ext[0] = adv_len;
    hd_ext[1] = flags;
    
    adv_len ++;
    hd[0+1] = adv_type;
    hd[1+1] = adv_len; 
        
    //aux adv ind
    flags = HDR_FLAG_ADI | HDR_FLAG_SYNCINFO |HDR_FLAG_TXPOWER;
//    if((g_aux_phy_sel == MG_ADV_PHY_500K) || (g_aux_phy_sel == MG_ADV_PHY_125K))
    {
        if(adva_enabled)flags |= HDR_FLAG_ADVA; //HDR_FLAG_TXPOWER, HDR_FLAG_TARGETADDR  
        if(adv_tx_power_enabled)flags |= HDR_FLAG_TXPOWER;
    }
    adv_len = get_extended_header_len(flags);//may ACAD part added if any.   ===> Extended Header Length
    if(acad2_len && acad2)adv_len += acad2_len;  
    hd_ext[2] = adv_len;
    hd_ext[3] = flags;
    
    adv_len ++;
    if((255 - adv_len) >= g_adv_data_pkt2_len)
    {
        adv_len += g_adv_data_pkt2_len;
    }
    else//buffer overflow
    {
        adv_len = 255;
    }
    hd[2+1] = adv_type;
    hd[3+1] = adv_len;
    
    //aux sync ind
    flags = 0;// | HDR_FLAG_TXPOWER
    if(adv_tx_power_enabled)flags |= HDR_FLAG_TXPOWER;
    if(g_aoa_enable_flag) flags |= HDR_FLAG_CTEINFO; //HDR_FLAG_TXPOWER  
    adv_len = get_extended_header_len(flags);//may ACAD part added if any.  ===> Extended Header Length
    if(acad3_len && acad3)adv_len += acad3_len;
    hd_ext[4] = adv_len;
    hd_ext[5] = flags;
    
    adv_len ++;
    if((255 - adv_len) >= g_adv_data_pkt3_len)
    {
        adv_len += g_adv_data_pkt3_len;
    }
    else//buffer overflow
    {
        adv_len = 255;
    }
    hd[4+1] = adv_type;
    hd[5+1] = adv_len;
    
    MG_Write_Buffer(ADV_PKT_CFG, hd,7); //adv header
    MG_Write_Buffer(EXT_HEADER_CFG1, hd_ext,6);//extended header
        
    //acad data if any
    if(acad2_len && acad2)MG_Write_Buffer(EXT_HEADER_CFG4, acad2,acad2_len);
    if(acad3_len && acad3)MG_Write_Buffer(EXT_HEADER_CFG5, acad3,acad3_len);
    
    //aux adv ind data
    if(g_adv_data_pkt2_len)fill_fifo_data(0,g_adv_data_pkt2,g_adv_data_pkt2_len);
    //aux chain ind data
    if(g_adv_data_pkt3_len)fill_fifo_data(1,g_adv_data_pkt3,g_adv_data_pkt3_len);
    
    set_sync_ind_info(); //ptr,aa, and sync_counter,...
    
    //parameter
    //g_adv_interval ==> sync interval
    //g_interval_2_sync_step_num,
    //g_sync_offset1_2_aux,
    //g_sync_offset2_2_sync       
    update_aux_sync_ind_par();//update rc
}

void set_radio_phy(void)
{
    u8 tmp;
    
    MG_Read_Buffer(BASEBAND_CFG,&tmp,1);
    
    tmp &= 0x07;
    if((g_aux_phy_sel == MG_ADV_PHY_500K) || (g_aux_phy_sel == MG_ADV_PHY_125K))
    {
        tmp |= (0x01 << 5);
        
        if(MG_ADV_PHY_500K == g_aux_phy_sel)tmp |= (0x01 << 3);
    } 
    else
    {
    }
 
    MG_Write_Buffer(BASEBAND_CFG,&tmp,1);
}
///
void set_primary_adv_channel_map(u8 map) /*MAP_CHANNEL_xx_BIT*/
{
    u8 tmp[4];
    
    map ^= 0xE0;//change to mask bit value
    
    MG_Read_Buffer(ADV_INTERV_RC_CFG,tmp,4);
    tmp[3] &= 0x1E;
    tmp[3] |=  (map);
    
    MG_Write_Buffer(ADV_INTERV_RC_CFG,tmp,4);
}

void set_ADI_AoA_par(void)
{
    u8 adi_info[2];
 //   u8 cte_info; //AoA Constant Tone Extension
    u8 tmp[7];
    
    //cte_info = g_cte_length | (0x00 << 6); //AoA Constant Tone Extension
    
    adi_info[0] = g_adi_did & 0xFF;
    adi_info[1] = (g_adi_did >> 8) | (g_adi_sid << 4);
    
    tmp[0] = adi_info[0];
    tmp[1] = adi_info[1];
    tmp[2] = adi_info[0];
    tmp[3] = adi_info[1];
    tmp[4] = adi_info[0];
    tmp[5] = adi_info[1];
    tmp[6] = 0; //no aoa
   
    MG_Write_Buffer(EXT_HEADER_CFG2,tmp,7);
}
///
void radio_setIrqMask(u8 mask)
{
    MG_Write_Buffer(INT_MASK,&mask,1);
}
///
void radio_clearIRQ(u8 irq_status)
{
    MG_Write_Buffer(INT_FLAG,&irq_status,1);
}

u8 radio_getIrqState(void)
{
    u8 tmp;
    
    MG_Read_Buffer(INT_FLAG,&tmp,1);
    
    if(tmp) MG_Write_Buffer(INT_FLAG,&tmp,1);
    
    return tmp;
}

/*******************************************************************************
* Function   :     	BLE_TX_Ext
* Parameter  :     	
* Returns    :     	void
* Description:
* Note:      :
*******************************************************************************/
void BLE_TX_Ext(void)
{   
    set_radio_phy();
    //set_primary_adv_channel_map(MAP_CHANNEL_37_BIT|MAP_CHANNEL_38_BIT|MAP_CHANNEL_39_BIT);
    set_ADI_AoA_par();
    
    //adv ext ind + aux adv ind + aux chain
    g_pkt_types = ADV_EXTEND_IND_BIT;
    
    switch (g_adv_ext3_type)
    {
        case MG_TYPE_ADV_NOT_USED: //adv ext ind + aux adv ind case(with big size of adv data)
            g_pkt_types |= AUX_ADV_IND_BIT;
            set_adv_ext_ind_par(TYPE_ADV_EXT_IND|TX_ADDR_TYPE,ACAD2,ACAD2_len);
            break;
        case MG_TYPE_AUX_CHAIN_IND: //adv ext ind + aux adv ind + aux chain ind case
            g_pkt_types |= (AUX_ADV_IND_BIT | AUX_CHAIN_IND_BIT);
            set_aux_chain_ind_par(TYPE_ADV_EXT_IND|TX_ADDR_TYPE,ACAD2,ACAD2_len);
            break;
        case MG_TYPE_AUX_SYNC_IND:  //adv ext ind + aux adv ind + aux sync ind(CTE) case
            g_pkt_types |= (AUX_ADV_IND_BIT | AUX_SYNC_IND_BIT);
            set_aux_sync_ind_par(TYPE_ADV_EXT_IND|TX_ADDR_TYPE,ACAD2,ACAD2_len,ACAD3,ACAD3_len);
            break;
        default:    //error
            break;
    }

    MG_Write_Reg(BASEBAND_CFG, 5); //SLP_XO=4, STOP=2, WKUP_EN=1
    MG_Write_Reg(WAKEUP_NOW, 1);
    
}

///////////////////////////////////COMMON func////////////////////////////////////
#if 1
unsigned char Patch_Data5_V4x[2] = {0xa0,0x20};
unsigned char Patch_Data6_V4x[2] = {0x03,0x02};
unsigned char Patch_Data7_V4x[4] = {0x40,0x08,0x03,0x55};

unsigned char Patch_Data0_V43[4] = {0x25,0xf4,0x33,0x55};
unsigned char Patch_Data1_V43[4] = {0xb1,0x14,0x33,0x55};
unsigned char Patch_Data2_V43[4] = {0xc0,0x0c,0x33,0x55};
unsigned char Patch_Data3_V43[4] = {0xc1,0x73,0x33,0x55};
unsigned char Patch_Data4_V43[4] = {0xc4,0xe4,0x33,0x55};
unsigned char Patch_Data5_V43[4] = {0xc5,0x2c,0x33,0x55};
unsigned char Patch_Data6_V43[4] = {0xc7,0x60,0x33,0x55};
unsigned char Patch_Data7_V43[4] = {0xb0,0xb0,0x33,0x55};
#endif

void patch(unsigned char ver)
{
    uint8_t data_buf[4] = {0x31,0x08,0x08,0x08};
    
#if (COMM_OPT_SEL == COMM_SPI)
    MG_Write_Buffer(0x3B, data_buf, 4); //csn,sck,miso input no pull
#endif

    //common patch
    MG_Write_Buffer(BG_CAL, Patch_Data5_V4x, 2);
    MG_Write_Buffer(XO_SET, Patch_Data6_V4x, 2);
    MG_Write_Buffer(ABUS_ACC, Patch_Data7_V4x, 4);
    
    if(0x42 < ver)
    {
        MG_Write_Buffer(ABUS_ACC, Patch_Data0_V43, 4);
        MG_Write_Buffer(ABUS_ACC, Patch_Data1_V43, 4);
        MG_Write_Buffer(ABUS_ACC, Patch_Data2_V43, 4);
        MG_Write_Buffer(ABUS_ACC, Patch_Data3_V43, 4);
        MG_Write_Buffer(ABUS_ACC, Patch_Data4_V43, 4);
        MG_Write_Buffer(ABUS_ACC, Patch_Data5_V43, 4);
        MG_Write_Buffer(ABUS_ACC, Patch_Data6_V43, 4);
        
        MG_Read_Buffer(BG_CAL, data_buf, 4);
        //if (data_buf[3] & 0x20)
        if(ValBit(data_buf[3],5))
        {
            Patch_Data7_V43[1] = ((data_buf[3] & 0x1f)<<2) | 0x80;
        }
        MG_Write_Buffer(ABUS_ACC, Patch_Data7_V43, 4);
    }
}

/*******************************************************************************
* Function   :     	BLE_Init
* Parameter  :     	void
* Returns    :     	void
* Description:      power on .BLE must initnal reg .
* Note:      :
*******************************************************************************/
void BLE_Init(void)
{
    uint8_t status = 0;
    uint8_t data_buf[5];
//    uint8_t ble_Addr[6];

    MG_Write_Reg(0x3e, 0x01); //rcosc_pd=0
    do{
#if (COMM_OPT_SEL == COMM_SPI)
        MG_Write_Reg(CONT_TEST_MODE, 8);//3wire spi. No use for 4wire spi
#endif
        status = MG_Read_Reg(CHIP_READY);
    }while((status == 0xFF) || (status&0x4) != 0x04);
    
    MG_Write_Reg(RESET_BB, 0x1);//clear flag
    MG_Write_Reg(RESET_BB, 0x0);

    MG_Write_Reg(RESET_BB, 0xDC);//reset bb
    do{
        status = MG_Read_Reg(RESET_BB);
    }while((status&0x2) != 0x02); //reset done
    MG_Write_Reg(RESET_BB, 0x1);//clear flag
    MG_Write_Reg(RESET_BB, 0x0);

    MG_Write_Reg(XO_PD_EN, 0x1); //3//rc32k_sel, xo_pd_en
#if 1
    data_buf[4] = 0x30; //xo_pd_wait_adjust
    data_buf[3] = 0x1e;
    data_buf[2] = 0x29;
    data_buf[1] = 0x05;
    data_buf[0] = 0xdb;
    MG_Write_Buffer(XO_WAIT,data_buf,5);
#endif
    //apply patch
    status = MG_Read_Reg(IC_VER);
    patch(status);

#if 0 //debug
    Uart_Send_String("chip version=");
    Uart_Send_Byte(status);
    Uart_Send_String("\r\n");
    
    MG_Read_Buffer(0x16, ble_Addr, 6);
    Uart_Send_String("BleAddr16=");
    Uart_Send_Byte(ble_Addr[5]);
    Uart_Send_Byte(ble_Addr[4]);
    Uart_Send_Byte(ble_Addr[3]);
    Uart_Send_Byte(ble_Addr[2]);
    Uart_Send_Byte(ble_Addr[1]);
    Uart_Send_Byte(ble_Addr[0]);
    Uart_Send_String("\r\n");
#endif

#if 1  //dcdc
    data_buf[0] = MG_Read_Reg(PA_CFG);
    if(!ValBit(data_buf[0],0)) //DcDc on
    {
        data_buf[4] = 0x03; //buck_dly_set
        data_buf[3] = 0x90; //buck_dly_set
        data_buf[2] = 0xb0;
        data_buf[1] = 0x30;
        data_buf[0] = 0xe0;
        MG_Write_Buffer(0x17,data_buf,5);
    }
#endif

    //set BLE TX Power
    data_buf[0] = 0x08;
    data_buf[1] = BLE_TX_POWER; //max 1F
    MG_Write_Buffer(PA_CFG, data_buf, 2);

    MG_Write_Reg(CONFIG, 0x87); //PREAMBLE_LEN_AUTO | SCRAMBLE_ON | CRC_EN | POWER_UP
    MG_Write_Reg(INT_MASK, 0x6A); //sleep int all
}
